
#include <winsock2.h>
#include <stdlib.h>
#include <string.h>
#include "sma_frame.h"
#include "crc32.h"
#include "smaAPI.h"

typedef struct _SmaApiInfo
{
	HANDLE h_mutex;
	WSADATA wsa_data;
	SOCKET socket_mcp;
	SOCKET socket_vsp;
	unsigned long frame_id;
} SmaApiInfo;

/* ---------------------------------------------------------------------- */

static int smaProcessFrameBody(unsigned char dst_id,
							   unsigned long send_len,
							   const unsigned char *send_buff,
							   unsigned long recv_size,
							   unsigned long *recv_len,
							   unsigned char *recv_buff);

static int smaProcessFrame(unsigned char dst_id,
						   unsigned long send_len,
						   const unsigned char *send_buff,
						   unsigned long recv_size,
						   unsigned long *recv_len,
						   unsigned char *recv_buff);

static unsigned long smaGetNextFrameId(void);

static int smaProcessCommand(unsigned char dst_id, 
							 unsigned char cmd_type, 
							 unsigned short cmd_code, 
							 unsigned long send_len,
							 const unsigned char *send_data,
							 unsigned long recv_size,
							 unsigned long *recv_len,
							 unsigned char *recv_data);

static SmaApiInfo api_info;

/* ---------------------------------------------------------------------- */

int smaInitLibrary(void)
{
	int ret;

	generate_crc_table();

	memset(&api_info, 0, sizeof(api_info));

	api_info.socket_mcp = INVALID_SOCKET;
	api_info.socket_vsp = INVALID_SOCKET;

	api_info.h_mutex = CreateMutex(NULL, FALSE, NULL);
	if(api_info.h_mutex == NULL)
	{
		return -1;
	}

	ret = WSAStartup(MAKEWORD(2, 2), &api_info.wsa_data);
	if(ret != 0)
	{
		CloseHandle(api_info.h_mutex);
		return -1;
	}

	return 0;
}

int smaDeinitLibrary(void)
{
	int ret;
	int error;

	error = 0;

	ret = WSACleanup();
	if(ret != 0)
	{
		error = 1;
	}

	CloseHandle(api_info.h_mutex);

	return error ? -1 : 0;
}

int smaConnect(enum SmaProcessor Processor, const char *Address, unsigned short Port)
{
	int ret;
	SOCKET client_socket;
	struct sockaddr_in addr;

	if((Processor != MCP) && (Processor != VSP))
	{
		return -1;
	}

	client_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if(client_socket == INVALID_SOCKET)
	{
		return -1;
	}

	memset(&addr, 0, sizeof(struct sockaddr_in));
	addr.sin_family = AF_INET;
	addr.sin_addr.s_addr = inet_addr(Address);
	addr.sin_port = htons(Port);

	ret = connect(client_socket, (struct sockaddr*)&addr, sizeof(struct sockaddr_in));
	if(ret != 0)
	{
		closesocket(client_socket);
		return -1;
	}

	WaitForSingleObject(api_info.h_mutex, INFINITE);

	if(Processor == MCP)
	{
		api_info.socket_mcp = client_socket;
	}
	else if(Processor == VSP)
	{
		api_info.socket_vsp = client_socket;
	}
	else
	{
		;
	}

	ReleaseMutex(api_info.h_mutex);

	return 0;
}

int smaDisconnect(enum SmaProcessor Processor)
{
	SOCKET *client_socket;
	
	if(Processor == MCP)
	{
		client_socket = &api_info.socket_mcp;
	}
	else if(Processor == VSP)
	{
		client_socket = &api_info.socket_vsp;
	}
	else
	{
		return -1;
	}

	WaitForSingleObject(api_info.h_mutex, INFINITE);


	shutdown(*client_socket, SD_BOTH);
	closesocket(*client_socket);

	*client_socket = INVALID_SOCKET;

	ReleaseMutex(api_info.h_mutex);

	return 0;
}

int ESMnoOp(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0001;
	send_len = 0;
	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int ESMsafe(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0002;
	send_len = 0;
	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaExitSafe(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0003;
	send_len = 0;
	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

void ESMreset(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0004;
	send_len = 0;
	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return;
}

int ESMabort(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0005;
	send_len = 0;
	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

static char sma_version_string[1025];

const char * ESMgetSwVersion(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len_mcp;
	unsigned long recv_len_vsp;
	unsigned char *recv_data;
	int error;

	error = 0;

	recv_data = (unsigned char *)&sma_version_string[0];

	cmd_type = 0x01;
	cmd_code = 0x0006;
	send_len = 0;
	recv_size = 512;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len_mcp, &recv_data[0]);
	if(ret < 0)
	{
		error = 1;
		recv_len_mcp = 0;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len_vsp, &recv_data[recv_len_mcp]);
	if(ret < 0)
	{
		error = 1;
		recv_len_vsp = 0;
	}

	/* Put null termination at the end of string. */
	recv_data[recv_len_mcp + recv_len_vsp] = '\0';
	
	if(error != 0)
	{
		recv_data = 0;
	}

	return (char *)recv_data;
}

int smaStartSelfTest(int bOnlyPrimarySelfTest)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[1];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x01;
	cmd_code = 0x0008;

	send_len = 1;
	send_data[0] = bOnlyPrimarySelfTest ? 1 : 0;

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaInquireTestProgress(int *bDone, int *bPassed, char Msg[512])
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;

	error = 0;
	*bDone = 0;
	*bPassed = 0;

	recv_data = (unsigned char *)malloc(258);

	cmd_type = 0x01;
	cmd_code = 0x0009;

	send_len = 0;

	recv_size = 258;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	
	if(recv_len < 2)
	{
		error = 1;
		len = 0;
	}
	else
	{
		len = recv_len - 2;
		*bDone = recv_data[0];
		*bPassed = recv_data[1];

		memcpy(&Msg[0], &recv_data[0], len);
	}

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 2)
	{
		error = 1;
	}
	else
	{
		*bDone = *bDone && recv_data[0];
		*bPassed = *bPassed && recv_data[1];

		memcpy(&Msg[len], &recv_data[0], recv_len - 2);
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaGetTelemetryMCP(SmaJointCfg *IncrEncoders, SmaJointCfg *AbsSensors, 
					   double FtSensors[6], double MotSpeeds[6], 
					   double MotCurrents[6], SmaMcpStatus *Statuses, 
					   char Msg[512])
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;

	error = 0;

	recv_data = (unsigned char *)malloc(669);

	cmd_type = 0x02;
	cmd_code = 0x0021;

	send_len = 0;

	recv_size = 669;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 157)
	{
		error = 1;
	}
	else
	{
		smaUnpackJointCfg(&recv_data[0], IncrEncoders); /* Offset 0: Incremental Encoders */

		smaUnpackJointCfg(&recv_data[24], AbsSensors); /* Offset 24: Absolute Encoders. */

		smaUnpackJointData(&recv_data[48], FtSensors); /* Offset 48: Force/Torque Sensors. */

		smaUnpackJointData(&recv_data[72], MotSpeeds); /* Offset 72: Motor Speeds. */

		smaUnpackJointData(&recv_data[96], MotCurrents); /* Offset 96: Force/Torque Sensors. */

		smaUnpackMcpStatus(&recv_data[120], Statuses); /* Offset 120: MCP status. */

		len = recv_len - 157;
		memcpy(&Msg[0], &recv_data[157], len); /* Offset 157: Message. */
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaGetTelemetryVSP(int TmStates[10], int *ImgCapState, enum  SmaDevStatus 
					   *CamState, int *nProgress, char Msg[512])
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;
	int i;

	error = 0;

	recv_data = (unsigned char *)malloc(525);

	cmd_type = 0x02;
	cmd_code = 0x0022;

	send_len = 0;

	recv_size = 525;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 13)
	{
		error = 1;
	}
	else
	{
		for(i=0; i<10; i++) /* Offset 0: State of task manager. */
		{
			TmStates[i] = (char)recv_data[i];
		}

		*ImgCapState = (char)recv_data[10]; /* Offset 10: State of image capturer. */

		*CamState = (char)recv_data[11]; /* Offset 11: State of camera. */

		*nProgress = (char)recv_data[12]; /* Offset 12: Progress. */

		len = recv_len - 13;
		memcpy(&Msg[0], &recv_data[13], len); /* Offset 13: Message. */
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaComputeFwdKinematics(SmaPose EePose, SmaJointCfg *q)
{
	return -1;
}

int smaComputeInvKinematics(SmaJointCfg q, SmaPose *EePose)
{
	return -1;
}

int smaTransformPose(SmaPose from, enum SmaRefFrame FromFrame, 
					 enum SmaRefFrame ToFrame, SmaPose *to)
{
	return -1;
}

int smaPlanPath(SmaPose from, SmaPose to, int bInJointSpace, 
				int bCameraConstraint, double fPercentMaxSpeed, 
				double fPercentMaxAcc, SmaPath *path)
{
	return -1;
}

int smaTeleOpArmJoint(enum SmaJoint Joint, enum SmaJntPosnType posn_type, double position)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[6];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;
	float data;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0031;

	send_len = 6;

	send_data[0] = Joint; /* Offset 0: Joint. */
	send_data[1] = posn_type; /* Offset 1: Position type. */

	data = (float)position;
	smaPackDword(&send_data[2], *(unsigned long *)&data); /* Offset 2: Position. */

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaTeleOpArmEndEffector(enum SmaRefFrame TaskFrame, SmaPose pose, int bStraight)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[26];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0032;

	send_len = 26;

	send_data[0] = TaskFrame; /* Offset 0: Task frame. */
	
	smaPackPose(&send_data[1], &pose); /* Offset 1: Pose. */

	send_data[25] = bStraight; /* Offset 25: bStraight. */

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaMoveArmToARP(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0033;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaHomeArm(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0034;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaStowArm(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0035;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaChangeDrillTool(void)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x03;
	cmd_code = 0x0036;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

void smaPackPoint3Dd(unsigned char *buff, const SmaPoint3Dd *point)
{
	float data;

	data = (float)point->x;
	smaPackDword(&buff[0], *(unsigned long *)&data); /* Offset 0: X. */

	data = (float)point->y;
	smaPackDword(&buff[4], *(unsigned long *)&data); /* Offset 4: Y. */

	data = (float)point->z;
	smaPackDword(&buff[8], *(unsigned long *)&data); /* Offset 8: Z. */
}

int smaConstructTgtMdl(SmaPoint3Dd AoiCenter, double AoiRadius, 
					   SmaPoint3Dd AoiNormal, double SurveyHeight)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[32];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;
	float data;

	error = 0;

	cmd_type = 0x04;
	cmd_code = 0x0051;

	send_len = 32;

	smaPackPoint3Dd(&send_data[0], &AoiCenter); /* Offset 0: AOI center. */

	data = (float)AoiRadius;
	smaPackDword(&send_data[12], *(unsigned long *)&data); /* Offset 12: AOI radius. */ 

	smaPackPoint3Dd(&send_data[16], &AoiNormal); /* Offset 16: AOI normal. */

	data = (float)SurveyHeight;
	smaPackDword(&send_data[28], *(unsigned long *)&data); /* Offset 28: Survey height. */ 

	recv_size = 1;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 0)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaDownloadImage(int ImgIdx, SmaImage *Image, SmaPose *CameraPose)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[2];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;

	error = 0;

	recv_data = (unsigned char *)malloc(1447710);

	cmd_type = 0x04;
	cmd_code = 0x0052;

	send_len = 2;

	smaPackWord(&send_data[0], ImgIdx);

	recv_size = 1447710;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1447710)
	{
		error = 1;
	}
	else
	{
		smaUnpackPose(&recv_data[0], CameraPose); /* Offset 0: Camera Pose. */

		smaUnpackImage(&recv_data[24], Image); /* Offset 24: Image. */
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaDownloadFeatures(int ImgIdx, SmaFeatures *Features, SmaMatches *Matches)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[2];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;
	unsigned long remain;

	error = 0;

	recv_data = (unsigned char *)malloc(14347);

	cmd_type = 0x04;
	cmd_code = 0x0053;

	send_len = 2;

	smaPackWord(&send_data[0], ImgIdx);

	recv_size = 14347;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 11)
	{
		error = 1;
	}
	else
	{
		len = smaUnpackFeature(&recv_data[0], recv_len, Features); /* Offset 0: Detected features. */ 

		remain = recv_len - len;

		len = smaUnpackMatch(&recv_data[len], remain, Matches); /* Offset "len": Matched features. */
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaDownloadTgtMdl(SmaTargetModel *Model, SmaPose *ModelToArm)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;
	unsigned long remain;

	error = 0;

	recv_data = (unsigned char *)malloc(586);

	cmd_type = 0x04;
	cmd_code = 0x0054;

	send_len = 0;

	recv_size = 586;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 26)
	{
		error = 1;
	}
	else
	{
		len = smaUnpackTargetModel(&recv_data[0], recv_len, Model);

		remain = recv_len - len;

		if(remain < 24)
		{
			error = 1;
		}
		else
		{
			smaUnpackPose(&recv_data[len], ModelToArm);
		}
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaInquireTmcProgress(int *nProgress)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x04;
	cmd_code = 0x0055;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else
	{
		*nProgress = (char)recv_data[0];
	}

	return error ? -1 : 0;
}

int smaDetermineTgtPoint(SmaPoint2Di TgtPixel, const SmaPose *CameraPose,  
						 const SmaFeatures *Features, const SmaMatches *Matches,
						 const SmaTargetModel *Model, const SmaPose *ModelToArm, 
						 SmaPoint3Dd *TgtPosn, int NeighborIDs[3])
{
	return -1;
}

int smaCanProvidePreloadForce(SmaPose DrillingPose)
{
	return -1;
}

int smaStartVisualServoing(SmaPose TgtPose, const SmaPath *RefPath)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char *send_data;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x04;
	cmd_code = 0x0056;

	send_len = 31 + 24 * RefPath->nPoints;
	send_data = (unsigned char *)malloc(send_len);

	smaPackPose(&send_data[0], &TgtPose); /* Offset 0: Target pose. */

	smaPackPath(&send_data[24], RefPath); /* Offset 24: Path. */

	recv_size = 1;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 0)
	{
		error = 1;
	}

	free(send_data);

	return error ? -1 : 0;
}

int smaDownloadPlannedPath(SmaPath *RefPath)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char *recv_data;
	int error;
	unsigned long len;

	error = 0;

	recv_data = (unsigned char *)malloc(24583);

	cmd_type = 0x04;
	cmd_code = 0x0057;

	send_len = 0;

	recv_size = 24583;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len < 7)
	{
		error = 1;
	}
	else
	{
		len = smaUnpackPath(&recv_data[0], recv_len, RefPath); /* Offset 0: Path. */
		if(len != recv_len)
		{
			error = 1;
		}
	}

	free(recv_data);

	return error ? -1 : 0;
}

int smaInquireVsProgress(int *nProgress)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x04;
	cmd_code = 0x0058;

	send_len = 0;

	recv_size = 1;

	dst_id = DEVICE_ID_VSP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, 0, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else
	{
		*nProgress = (char)recv_data[0];
	}

	return error ? -1 : 0;
}

int smaStartDrillTgtTask(int bChangeTool, int bReturnSample, int bWithVS, int nPreloadForce)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[4];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x05;
	cmd_code = 0x0071;

	send_len = 4;

	send_data[0] = bChangeTool; /* Offset 0: bChangeTool. */
	send_data[1] = bReturnSample; /* Offset 1: bReturnSample. */
	send_data[2] = bWithVS; /* Offset 2: bWithVS. */
	send_data[3] = nPreloadForce; /* Offset 3: Preload force. */

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaStartObserveTgtTask(int bWithVS)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[1];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x05;
	cmd_code = 0x0072;

	send_len = 1;

	send_data[0] = bWithVS; /* Offset 0: bWithVS. */

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaNotifyTaskStatus(enum SmaProcessor ToProcessor, enum SmaTaskNotif nStatus, int param)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[3];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	if(ToProcessor == MCP)
	{
		dst_id = DEVICE_ID_MCP;
	}
	else if(ToProcessor == VSP)
	{
		dst_id = DEVICE_ID_VSP;
	}
	else
	{
		return -1;
	}

	cmd_type = 0x05;
	cmd_code = 0x0073;

	send_len = 3;

	send_data[0] = nStatus; /* Offset 0: Status. */
	
	smaPackWord(&send_data[1], param); /* Offset 1: Parameter. */

	recv_size = 1;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaManagePower(enum SmaPwrMgmtDevice Device, enum SmaPwrMgmtCmd Command)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[2];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	if(Device == Camera)
	{
		dst_id = DEVICE_ID_VSP;
	}
	else
	{
		dst_id = DEVICE_ID_MCP;
	}

	cmd_type = 0x06;
	cmd_code = 0x0091;

	send_len = 2;

	send_data[0] = Device; /* Offset 0: Device. */
	send_data[1] = Command; /* Offset 1: Command. */

	recv_size = 1;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

int smaRegulateCameraLight(int nPercentOn)
{
	int ret;
	unsigned char dst_id;
	unsigned char cmd_type;
	unsigned short cmd_code;
	unsigned long send_len;
	unsigned char send_data[1];
	unsigned long recv_size;
	unsigned long recv_len;
	unsigned char recv_data[1];
	int error;

	error = 0;

	cmd_type = 0x06;
	cmd_code = 0x0092;

	send_len = 1;

	send_data[0] = nPercentOn; /* Offset 0: Brightness level. */

	recv_size = 1;

	dst_id = DEVICE_ID_MCP;

	ret = smaProcessCommand(dst_id, cmd_type, cmd_code, send_len, send_data, recv_size, &recv_len, recv_data);
	if(ret < 0)
	{
		error = 1;
	}
	else if(recv_len != 1)
	{
		error = 1;
	}
	else if(recv_data[0] != 1)
	{
		error = 1;
	}

	return error ? -1 : 0;
}

/* ---------------------------------------------------------------------- */

static int smaProcessFrameBody(unsigned char dst_id,
							   unsigned long send_len,
							   const unsigned char *send_buff,
							   unsigned long recv_size,
							   unsigned long *recv_len,
							   unsigned char *recv_buff)
{
	int ret;
	SOCKET client_socket;

	if(dst_id == DEVICE_ID_MCP)
	{
		client_socket = api_info.socket_mcp;
	}
	else if(dst_id == DEVICE_ID_VSP)
	{
		client_socket = api_info.socket_vsp;
	}
	else
	{
		return -1;
	}

	if(client_socket == INVALID_SOCKET)
	{
		return -1;
	}

	ret = send(client_socket, (const char *)send_buff, send_len, 0);
	if(ret == SOCKET_ERROR)
	{
		return -1;
	}

	ret = recv(client_socket, (char *)recv_buff, recv_size, 0);
	if(ret == SOCKET_ERROR)
	{
		return -1;
	}

	*recv_len = ret;

	return 0;
}

static int smaProcessFrame(unsigned char dst_id,
						   unsigned long send_len,
						   const unsigned char *send_buff,
						   unsigned long recv_size,
						   unsigned long *recv_len,
						   unsigned char *recv_buff)
{
	int ret;

	WaitForSingleObject(api_info.h_mutex, INFINITE);

	ret = smaProcessFrameBody(dst_id, send_len, send_buff, recv_size, recv_len, recv_buff);

	ReleaseMutex(api_info.h_mutex);

	return ret;
}

static unsigned long smaGetNextFrameId(void)
{
	unsigned long frame_id;

	WaitForSingleObject(api_info.h_mutex, INFINITE);

	frame_id = api_info.frame_id++;

	ReleaseMutex(api_info.h_mutex);

	return frame_id;
}

static int smaProcessCommand(unsigned char dst_id, 
							 unsigned char cmd_type, 
							 unsigned short cmd_code, 
							 unsigned long send_len,
							 const unsigned char *send_data,
							 unsigned long recv_size,
							 unsigned long *recv_len,
							 unsigned char *recv_data)
{
	SmaFrameHeader header;
	int ret;
	unsigned long frame_id;
	unsigned long sent_time;
	unsigned long s_len;
	unsigned char *send_buff;
	unsigned long r_size;
	unsigned long r_len;
	unsigned char *recv_buff;
	int error;

	error = 0;

	if((dst_id != DEVICE_ID_MCP) && (dst_id != DEVICE_ID_VSP))
	{
		/* Destination device has to be either MCP or VSP. */
		return -1;
	}

	frame_id = smaGetNextFrameId();

	sent_time = GetTickCount();

	header.src_id = DEVICE_ID_API;
	header.dst_id = dst_id;
	header.frame_id = frame_id;
	header.sent_time = sent_time;
	header.cmd_type = cmd_type & 0x7F; /* Set bit7 to 0 to indicate request frame. */
	header.cmd_code = cmd_code;

	s_len = smaCalculateFrameSize(send_len);
	r_size = smaCalculateFrameSize(recv_size);

	send_buff = (unsigned char *)malloc(s_len);
	recv_buff = (unsigned char *)malloc(r_size);

	ret = smaPackDataFrame(s_len, send_buff, &header, send_len, send_data);
	if(ret < 0)
	{
		free(send_buff);
		free(recv_buff);
		return -1;
	}

	ret = smaProcessFrame(dst_id, s_len, send_buff, r_size, &r_len, recv_buff);
	if(ret < 0)
	{
		free(send_buff);
		free(recv_buff);
		return -1;
	}

	ret = smaUnpackDataFrame(r_len, recv_buff, &header, recv_size, recv_len, recv_data);
	if(ret < 0)
	{
		free(send_buff);
		free(recv_buff);
		return -1;
	}

	if(header.src_id != dst_id)
	{
		/* Source Device ID mismatch. */
		error = 1;
	}
	else if(header.dst_id != DEVICE_ID_API)
	{
		/* Destination Device ID mismatch. */
		error = 1;
	}
	else if((header.cmd_type & 0x80) == 0)
	{
		/* Bit7 should be 1 for reply frame. */
		error = 1;
	}
	else if((header.cmd_type & 0x7F) != (cmd_type & 0x7F))
	{
		/* Command Type mismatch. */
		error = 1;
	}
	else if(header.cmd_code != cmd_code)
	{
		/* Command Code mismatch. */
		error = 1;
	}
	else if(header.frame_id != frame_id)
	{
		/* Frame ID mismatch. */
		error = 1;
	}

	free(send_buff);
	free(recv_buff);

	return error ? -1 : 0;
}

/* ---------------------------------------------------------------------- */
